Occupancy Grid Mapping

Star Discuss Download

In this mapping method, the 2-D world to be mapped is divided into evenly spaced grids, each grid has one of the three possible states: free, occupied and unknown. The robot wandering around in the world and take measurements (e.g. lidar scan) along the way, to obtain the pair

, these pairs are used to generate the map.

The map is represented with a matrix, with each element corresponding to the state of a grid in the world. At each timestamp, the belief of state of the grid is updated according to binary (free vs occupied) Bayes filter using the old belief and the measurement (inverse measurement model i.e.

) (grid with unknown state is left untouched).

% Load the map, prerecorded poses and measurements.

% The world is 27 meters in width by 26 meters, and the robot drives along

% a pre-determined path in the world, and taking lidar scans along the way.

% Theese data are recorded using the Matlab example

% "MappingWithKnownPosesDiffDriveExample"

load('sensorData.mat');

% These are the robot's pose at each timestamp (x, y, theta),

% and 258 ranges and the corresponding angles (relative to the robot)

% at which the range is measured

robotPoses = allPoses;

sensorRanges = allScannedRanges';

sensorAngles = allScannedAngles';

% Use only part of the data

robotPoses = robotPoses(1:858, :);

sensorRanges = sensorRanges(1:858, :);

sensorAngles = sensorAngles(1:858, :);

% Small data for dev

% robotPoses = robotPoses(150:200, :);

% sensorRanges = sensorRanges(150:200, :);

% sensorAngles = sensorAngles(150:200, :);

% Show the reference map and the path on the left

refMap = binaryOccupancyMap(simpleMap,1);

subplot(1, 2, 1);

hold on

show(refMap);

path = [4 6; 6.5 12.5; 4 22; 12 14; 22 22; 16 12; 20 10; 14 6; 22 3];

plot(path(:,1),path(:,2), 'o-');

mapWidth = 27;

mapHeight = 26;

gridSize = 0.1;

% The belief represents the map,

% higher value means more likely to be occupied

cols = mapWidth / gridSize;

rows = mapHeight / gridSize;

belief = zeros([rows, cols]);

initialBelief = zeros([rows, cols]);

% Live plot for the mapping process

subplot(1, 2, 2);

% Loop over the (pose, measurement) pairs

tic;

[numSensorDatas, ~] = size(sensorRanges);

pbar = waitbar(0, "Mapping", 'Name', "Mapping...");

for i = 1:numSensorDatas

hold off;

progress = i / numSensorDatas;

waitbar(progress, pbar, sprintf("%.2f%%", progress * 100));

% Process one measurement

range = sensorRanges(i, :);

angle = sensorAngles(i, :);

pose = robotPoses(i, :);

% Loop over the the grids

% TODO: this is painfully slow

parfor r = 1:rows

for c = 1:cols

% Get the coordinate of the grid

x = (c - 1) * gridSize + gridSize / 2.0;

y = (rows - r) * gridSize + gridSize / 2.0;

% Calculate the occupancy state of the grid

newBelief = inverseSensorModel(x, y, range, angle, pose);

if isfinite(newBelief)

belief(r, c) = belief(r, c) + newBelief - initialBelief(r, c);

end

% Plot the current belief along with the location of the robot

p = 1 ./ (1 + exp(belief));

imshow(p);

hold on;

title("Constructed Map");

xlim([0, cols]);

ylim([0, rows]);

plot(path(:, 1) / gridSize, rows-path(:, 2) / gridSize, 'o-');

px = pose(1, 1) / gridSize;

py = rows - pose(1, 2) / gridSize;

plot(px, py, 'r+');

% Save to a gif

frame = getframe(gcf);

img = frame2im(frame);

[img,cmap] = rgb2ind(img, 256);

if i == 1

imwrite(img,cmap, 'occupancyGridMapping.gif', 'gif', 'LoopCount', Inf, 'DelayTime', 1/60);

else

imwrite(img,cmap, 'occupancyGridMapping.gif', 'gif', 'WriteMode', 'append', 'DelayTime', 1/60);

end

toc;

Elapsed time is 720.287976 seconds.

close(pbar);

function negLogOdds = inverseSensorModel(xi, yi, range, angle, pose)
    % range of the lidar sensor
    zMax = 10;
    zMin = 0;
    
    % special value to indicate unknown state
    lUnknown = inf;
    
    lFree = -0.4;
    lOccupied = 0.4;
    
    % size of the grid
    alpha = 0.1;
    
    % remove invalid scans before doing anything interesting
    good = find(isfinite(range) & range >= zMin & range <= zMax);
    ranges = range(:, good);
    angles = angle(:, good);
    
    x = pose(1, 1);
    y = pose(1, 2);
    theta = pose(1, 3);
    
    % distance between the robot and grid
    r = norm([xi-x, yi-y]);
    
    % angle of the grid (xi, yi) relative to the robot
    phi = atan2((yi-y), (xi-x)) - theta;
    
    % find the closest sensor to the grid
    [minAngle, k] = min(abs(angles - phi));
    zK = ranges(1, k);
    
    % determine the state
    if minAngle >= pi /2 ...
        || (r > alpha / 2 && tan(minAngle) > (alpha / 2) / (r - alpha / 2))
        negLogOdds = lUnknown;
    elseif abs(r - zK) <= alpha / 2
        negLogOdds = lOccupied;
    elseif r < zK - alpha / 2
        negLogOdds = lFree;
    else
        negLogOdds = lUnknown;
    end
end